#include "identtrackertemplate.h"
#include "preprocessor.h"
#include "source/configmanager.h"
#include "source/logger.h"

IdentTrackerTemplate::IdentTrackerTemplate(float scaleFactor) : IdentTracker(scaleFactor), minDistance(DBL_MAX)
{
    // fitThreshold = Pama
}

void IdentTrackerTemplate::initialize(const cv::Mat &templateImg, const cv::Rect& roi)
{
    LogDebug << "Initializing IdentTrackerTemplate";
    this->templateImg = templateImg.clone();
    bestIdentifiedRect = roi; // Reset best identified rect
    minDistance = DBL_MAX; // Reset min distance
    LogDebug << "Starting initialize. Frame size: " << templateImg.cols << "x" << templateImg.rows
             << ", Template ROI size: " << roi;
}

// void IdentTrackerTemplate::setFitThreshold_(float fitThreshold_)
// {
//     fitThreshold =fitThreshold_;
// }

cv::Rect2d IdentTrackerTemplate::detectAndMatch(const cv::Mat &frame,float &Similarity)
{
    LogTrack << "Starting detectAndMatch. Frame size: " << frame.cols << "x" << frame.rows << "h" << frame.channels()
             << ", Template ROI size: " << templateImg.cols << "x" << templateImg.rows <<  "h" << templateImg.channels() ;

    // 检查模板图像的尺寸是否大于被匹配图像的尺寸
    int64 start = cv::getTickCount();
    if (templateImg.rows > frame.rows || templateImg.cols > frame.cols)
    {
        LogError << "Template size is larger than the frame size. Template: "
                 << templateImg.cols << "x" << templateImg.rows
                 << ", Frame: " << frame.cols << "x" << frame.rows;
        int64 end = cv::getTickCount();
        double time = (end - start) / cv::getTickFrequency() * 1000.0;
        LogError << "Execution time: " << time << " ms";
        return cv::Rect2d();
    }

    // 执行模板匹配
    int64 startMatch = cv::getTickCount();
    cv::Mat result;
    cv::matchTemplate(frame, templateImg, result, cv::TM_CCOEFF_NORMED);
    int64 endMatch = cv::getTickCount();
    double timeMatch = (endMatch - startMatch) / cv::getTickFrequency() * 1000.0;
    LogTrack << "Template matching completed. Result size: " << result.cols << "x" << result.rows;

    // 打印执行时间
    double totalTime = (endMatch - start) / cv::getTickFrequency() * 1000.0;
    LogTrack << "Total execution time: " << totalTime << " ms";
    LogTrack << "Template size: " << templateImg.cols << "x" << templateImg.rows
             << ", Frame size: " << frame.cols << "x" << frame.rows;

    std::vector<cv::Point> locations;
    std::vector<double> values;

    fitThreshold =  PreProcessor::getInstance().fitThreshold();
    // 记录匹配位置和相似度值
    for (int i = 0; i < result.rows; i++)
    {
        for (int j = 0; j < result.cols; j++)
        {
            double similarity_ = result.at<float>(i, j);
            // if (similarity_ > max_similarity_)
            // {
            //     max_similarity_ = similarity_;
            // }
            if (similarity_ >= fitThreshold)
            {
                locations.push_back(cv::Point(j, i));
                values.push_back(similarity_);
                //LogDebug << "Found match at (" << j << ", " << i << ") with similarity: " << similarity;
            }
        }
    }

    //--------------------------V2---------------------------//
    if (locations.empty())
    {
        return cv::Rect2d();
    }

    // 计算聚类阈值，基于模板的宽度和预设的比例（70%）
    cv::Rect2d identROI = PreProcessor::getInstance().identImg_.getRoi();
     cv::Rect2d finalMonitorROI = PreProcessor::getInstance().monitorImg_.getRoi();

    double clusterThreshold =identROI.width
                              * PreProcessor::getInstance().scaleFactor() * 0.7;

    // 聚类匹配点
    std::vector<std::vector<int>> clusters; // 存储每个类的匹配点索引
    std::vector<bool> visited(locations.size(), false);

    for (size_t i = 0; i < locations.size(); i++)
    {
        if (visited[i]) continue; // 如果该点已经被归类，跳过

        std::vector<int> cluster;
        cluster.push_back(i);
        visited[i] = true;

        // 对当前点进行聚类
        for (size_t j = i + 1; j < locations.size(); j++)
        {
            if (visited[j]) continue;

            double distance = cv::norm(locations[i] - locations[j]);
            if (distance <= clusterThreshold)
            {
                cluster.push_back(j);
                visited[j] = true;
            }
        }
        clusters.push_back(cluster); // 完成一个类的聚类
    }

    double identROIX = (finalMonitorROI.width * 0.4 - 0.2 * identROI.width) * PreProcessor::getInstance().scaleFactor();
    double minDistanceToIdentROIX = std::numeric_limits<double>::max();
    int closestClusterIndex = -1;
    for (size_t i = 0; i < clusters.size(); i++)
    {
        // 计算聚类中与 identROIX 最近的点的距离
        for (int idx : clusters[i])
        {
            double distanceToIdentROI = std::abs(locations[idx].x - identROIX);
            if (distanceToIdentROI < minDistanceToIdentROIX)
            {
                minDistanceToIdentROIX = distanceToIdentROI;
                closestClusterIndex = i;
            }
        }
    }


    double bestSimilarity = 0;
    cv::Point bestLocation;

    if (closestClusterIndex != -1)
    {
        for (int idx : clusters[closestClusterIndex])
        {
            if (values[idx] > bestSimilarity)
            {
                bestSimilarity = values[idx];
                bestLocation = locations[idx];
            }
        }
    }

    // 绘制出最终选择的位置
    cv::Rect2d bestMatchRect(bestLocation.x, bestLocation.y, templateImg.cols, templateImg.rows);
    Similarity = bestSimilarity;

    // 可选：绘制矩形框标记匹配位置
    //cv::rectangle(frame, bestMatchRect, cv::Scalar(0, 255, 0), 2);

    LogTrack << "Best match found at (" << bestLocation.x << ", " << bestLocation.y << ") with similarity: " << bestSimilarity;

    return bestMatchRect;

//-----------------------------------V1---------------------------------
#if 0
    Similarity = max_similarity_;
    if (locations.empty())
    {
        LogWarning << "No matches found in IdentTrackerTemplate with sufficient similarity";
        return cv::Rect2d();
    }
    //防止偏转

    std::vector<int> indices(locations.size());
    std::iota(indices.begin(), indices.end(), 0);
    std::sort(indices.begin(), indices.end(), [&locations](int i1, int i2) {
        return locations[i1].x < locations[i2].x;
    });

    std::vector<cv::Point> sortedLocations(locations.size());
    std::vector<double> sortedValues(values.size());
    for (size_t i = 0; i < indices.size(); ++i)
    {
        sortedLocations[i] = locations[indices[i]];
        sortedValues[i] = values[indices[i]];
    }

    double minXDistance = PreProcessor::getInstance().monitorImg_.getRoi().width*0.7 * PreProcessor::getInstance().scaleFactor();

    std::vector<std::vector<cv::Point>> regions;
    std::vector<std::vector<double>> regionValues;

    regions.push_back({sortedLocations[0]});
    regionValues.push_back({sortedValues[0]});

    for (size_t i = 1; i < sortedLocations.size(); ++i)
    {
        bool addedToRegion = false;
        for (size_t j = 0; j < regions.size(); ++j)
        {
            if (std::abs(sortedLocations[i].x - regions[j].back().x) <= minXDistance)
            {
                regions[j].push_back(sortedLocations[i]);
                regionValues[j].push_back(sortedValues[i]);
                addedToRegion = true;
                break;
            }
        }
        if (!addedToRegion)
        {
            regions.push_back({sortedLocations[i]});
            regionValues.push_back({sortedValues[i]});
        }
    }

    cv::Rect2d identROI = PreProcessor::getInstance().identImg_.getRoi();
    double identROIX = (identROI.x + identROI.width / 2) * PreProcessor::getInstance().scaleFactor();

    // 找到与identROI X坐标最接近的区域
    int bestRegionIndex = -1;
    double minDistance = DBL_MAX;

    for (size_t i = 0; i < regions.size(); ++i)
    {
        double lastX = regions[i].back().x;
        double distance = std::abs(lastX - identROIX);
        if (distance < minDistance)
        {
            minDistance = distance;
            bestRegionIndex = i;
        }
    }


    const std::vector<cv::Point> &bestLocations = regions[bestRegionIndex];
    const std::vector<double> &bestValues = regionValues[bestRegionIndex];

    // 获取最高相似度的匹配位置
    auto maxElementIt = std::max_element(bestValues.begin(), bestValues.end());
    size_t maxIndex = std::distance(bestValues.begin(), maxElementIt);

    cv::Point bestMatchLoc = bestLocations[maxIndex];
    cv::Rect2d bestRect(bestMatchLoc.x, bestMatchLoc.y, templateImg.cols, templateImg.rows);
    Similarity = bestValues[maxIndex];

    // 更新 bestIdentifiedRect 和 prevRect
    bestIdentifiedRect = bestRect;
    minDistance = DBL_MAX; // 重置最小距离

    LogTrack << "detectAndMatch completed. Best rect: (" << bestIdentifiedRect.x << ", "
             << bestIdentifiedRect.y << ", " << bestIdentifiedRect.width << ", "
             << bestIdentifiedRect.height << ")";



    // // 获取前5个相似度最高的匹配位置
    // std::vector<size_t> indices(values.size());
    // std::iota(indices.begin(), indices.end(), 0);
    // std::sort(indices.begin(), indices.end(), [&values](size_t i1, size_t i2) { return values[i1] > values[i2]; });

    // std::vector<cv::Rect2d> candidates;

    // size_t min = std::min(indices.size(), size_t(PreProcessor::getInstance().optimalNumber()));
    // for (size_t i = 0; i < min; ++i)
    // {
    //     cv::Point loc = locations[indices[i]];
    //     candidates.emplace_back(loc.x, loc.y, templateImg.cols, templateImg.rows);
    //     // LogDebug << "Candidate " << i << ": (" << loc.x << ", " << loc.y << ") with similarity " << values[indices[i]];
    // }

    // // 选择与上一次检测位置最接近的候选区域
    // cv::Rect2d bestRect = findClosestIdentifiedRect(candidates, bestIdentifiedRect);
    // LogDebug << "Best matching rect: (" << bestRect.x << ", " << bestRect.y << ", "
    //          << bestRect.width << ", " << bestRect.height << ")";

    // // 更新 bestIdentifiedRect 和 prevRect
    // bestIdentifiedRect = bestRect;
    // minDistance = DBL_MAX; // 重置最小距离

    // LogDebug << "detectAndMatch completed. Best rect: (" << bestIdentifiedRect.x << ", "
    //          << bestIdentifiedRect.y << ", " << bestIdentifiedRect.width << ", "
    //          << bestIdentifiedRect.height << ")";

    return bestIdentifiedRect;
}

cv::Rect2d IdentTrackerTemplate::findClosestIdentifiedRect(const std::vector<cv::Rect2d>& candidates, const cv::Rect2d& prevRect)
{
    LogDebug << "Starting findClosestIdentifiedRect. Previous rect: (" << prevRect.x << ", "
             << prevRect.y << ", " << prevRect.width << ", " << prevRect.height << ")";

    cv::Rect2d closestRect;
    double minDist = DBL_MAX;

    cv::Point2f prevCenter(prevRect.x + prevRect.width / 2, prevRect.y + prevRect.height / 2);

    for (const auto& rect : candidates)
    {
        cv::Point2f currCenter(rect.x + rect.width / 2, rect.y + rect.height / 2);
        double distance = cv::norm(prevCenter - currCenter);
        LogDebug << "Candidate rect: (" << rect.x << ", " << rect.y << ", "
                 << rect.width << ", " << rect.height << ") with distance: " << distance;

        if (distance < minDist)
        {
            minDist = distance;
            closestRect = rect;
        }
    }

    LogDebug << "Closest rect: (" << closestRect.x << ", " << closestRect.y << ", "
             << closestRect.width << ", " << closestRect.height << ") with distance: " << minDist;

    return closestRect;
#endif
}
